#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "fitSphere.h"

inline static double square(double num) {
	return num*num;
}

int FartherThan(Point p1,Point p2,double distanceSquare) {
	return square(p1.x-p2.x) + square(p1.y-p2.y) + square(p1.z-p2.z) > distanceSquare;
}

void printMatrix(double matrix[N][N+1], int rows, int cols) {
    for (int i = 0; i < rows; i++) {
        for (int j = 0; j < cols; j++) {
            printf("%lf ", matrix[i][j]);
        }
        printf("\n");
    }
}

int SolveQuaternion(double matrix[N][N+1],double *result) {
    // Forward Elimination
    for (int i = 0; i < N; i++) {
        // Find the pivot element in the current column
        double maxEl = fabs(matrix[i][i]);
        int maxRow = i;
        for (int k = i + 1; k < N; k++) {
            if (fabs(matrix[k][i]) > maxEl) {
                maxEl = fabs(matrix[k][i]);
                maxRow = k;
            }
        }

        // Swap the maximum row with the current row
        for (int k = i; k <= N; k++) {
            double temp = matrix[maxRow][k];
            matrix[maxRow][k] = matrix[i][k];
            matrix[i][k] = temp;
        }

        // Make all rows below this one 0 in current column
        for (int k = i + 1; k < N; k++) {
            if(fabs(matrix[i][i]) <= 0.0001)
                return 0;
            double c = -matrix[k][i] / matrix[i][i];
            for (int j = i; j <= N; j++) {
                if (i == j) {
                    matrix[k][j] = 0;
                } else {
                    matrix[k][j] += c * matrix[i][j];
                }
            }
        }
        
    }

    // Backward Substitution
    double x[N] = {0};
    for (int i = N - 1; i >= 0; i--) {
        if(fabs(matrix[i][i]) <= 0.0001)
                return 0;
        x[i] = matrix[i][N] / matrix[i][i];
        for (int k = i - 1; k >= 0; k--) {
            matrix[k][N] -= matrix[k][i] * x[i];
        }
    }
    for (int i = 0; i < N; i++) {
        result[i] = x[i];
    }
    return 1;
}

int fitSphere(Point p1,Point p2,Point p3,Point p4,Point *result) {
    double matrix[N][N+1] = {
        p1.x, p1.y, p1.z, 1, -p1.x*p1.x-p1.y*p1.y-p1.z*p1.z,
        p2.x, p2.y, p2.z, 1, -p2.x*p2.x-p2.y*p2.y-p2.z*p2.z,
        p3.x, p3.y, p3.z, 1, -p3.x*p3.x-p3.y*p3.y-p3.z*p3.z,
        p4.x, p4.y, p4.z, 1, -p4.x*p4.x-p4.y*p4.y-p4.z*p4.z,
    };
    //printMatrix(matrix, N, N+1);
    double p[4];
    if(!SolveQuaternion(matrix,p)) {
        //printf("No solution.\n");
        return 0;
    }
    double x = -p[0]/2, y = -p[1]/2, z = -p[2]/2;
		result->x = x;
		result->y = y;
		result->z = z;
    //double R = sqrt(x*x+y*y+z*z-p[3]);
    //printf("Sphere %.2lf(%.2lf,%.2lf,%.2lf)\n",x,y,z,R);
    return 1;
}
